/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief GIMnet connector class.
 * \author Harri Kukkonen
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * This class is responsible for maintaining lower level connection to
 * GIMnetAP. This class also takes care of receiving messages, as well
 * as handles passing them on the higher level.
 *
 * \version 1 By Antti, heavy modifications; ripping out most of lower levels
 * \version 0 By Harri
 *
 */

#include "connector.h"
#include "apsecurity.h"
#include "GIMnetAP/hubctrl_common.h"
#include "apmulticastframe.h"
#include "apnameserviceframe.h"
#include "fastProtocolHdr.h"
#include "gimicore.h"
#include "namecache.hpp"

#include <inttypes.h>



using namespace gimi;

//*****************************************************************************

Connector::Connector(GIMICore* aGIMICorePtr)
  : CSync(2,2),
    CThread(1),
    reconnectedStatus(),
    iOwnHubId(0x00000000),
    iOwnHubName(""),
    iHubAddress(""),
    iHubPort(0),
    iForceNameRegistration(false),
    iIsActive(false),
    iIsConnected(false),
    iProtocolDisp(),
    iGIMICorePtr(aGIMICorePtr),
    iHubCtrlSecretKey(NULL),
    iHubCtrlSecretKeySize(0),
    iHubCtrlMsgReplyBuffer(),
    iHubCtrlMessageSeq(0),
    iHubCtrlMessageValidySeconds(10*60),
    iNameServiceNodeId(0x00010001),
    iNameServiceMessageSeq(),
    iNameServiceOwnNameValidity(15*60),
    iNameServiceRefreshIntervalMs((iNameServiceOwnNameValidity*2000)/3), // 2/3 of the Validity time; converted to milliseconds
    iNameServiceNextRefreshTime(true, iNameServiceRefreshIntervalMs), // Now + interval
    iNameServiceMsgReplyBuffer(),
    iNameCache(),
    iMulticastNodeId(0x00010002),
    iMulticastMessageSeq(1),
    iMulticastMsgReplyBuffer(),
    iMulticastServiceDiscoveryGroupId(0x00000001),
    iMulticastServiceDiscoveryAccessKey(0x0000000000000000),
    iCongestionLevelArray(),
    iErrorArray()
{
  // Platform Sanity checks
  assert(sizeof(TFastProtocolHeader) == SIZEOF_TFASTPROTOCOLHEADER);
  assert(sizeof(TAPMulticastCtrl) == SIZEOF_TAPMULTICASTCTRL);
  assert(sizeof(TAPMulticastFrame) == SIZEOF_TAPMULTICASTFRAME);
  assert(sizeof(TAPNameServiceFrame) == SIZEOF_TAPNAMESERVICEFRAME);


  // Data-Init of msgreplybuf
  msgreplybufferInit(&iHubCtrlMsgReplyBuffer);

  msgreplybufferInit(&iNameServiceMsgReplyBuffer);
  msgreplybufferInit(&iMulticastMsgReplyBuffer);

  // Data-initialization of ProtocolDisp.
  const int v = protocolDispInit(&iProtocolDisp);
  assert(v == 0);

  
  // Register callbacks
  protocolDispRegisterCallback(&iProtocolDisp,
                               FASTPROTOCOL_NUMBER,
                               fastProtocolCallbackFunc,
                               this);
  protocolDispRegisterCallback(&iProtocolDisp,
                               HUBCTRLPROTOCOL_NUMBER,
                               hubctrlProtocolCallbackFunc,
                               this);

}
//*****************************************************************************

Connector::~Connector()
{
  CloseConnection();

  protocolDispDeinit(&iProtocolDisp);
  msgreplybufferDeinit(&iMulticastMsgReplyBuffer);
  msgreplybufferDeinit(&iNameServiceMsgReplyBuffer);
  msgreplybufferDeinit(&iHubCtrlMsgReplyBuffer);

  if (iHubCtrlSecretKey) {
    delete[] iHubCtrlSecretKey;
    iHubCtrlSecretKey = NULL;
  }

  
  // Force all to false finally here.
  iIsActive = iIsConnected = false;
}
//*****************************************************************************

void Connector::CloseConnection(void)
{
  if (iIsActive) {
    // We _really_ want to do this!
    ExecuteMulticastServiceUnsubscribe(2000);
    ExecuteNameServiceUnregisterName(2000);
    
    iIsConnected = false;
    TerminateThreads();
    protocolDispDisconnect(&iProtocolDisp);
  }

  // Clear namecache
  iNameCache.ClearNameCache();

}
//*****************************************************************************

void Connector::ExecuteThreads(void)
{
  if (iIsActive == false) {
    iIsActive = true;
    RunThread();
  }
}
//*****************************************************************************

void Connector::TerminateThreads(void)
{
  if (iIsActive == true) {
    iIsActive = false;
    
    SetRequestTermination();

    // Notify other, and Cancel the other.
    SignalThread(0, SIGINT);

    WaitThread();

  }
}
//*****************************************************************************

bool Connector::ExecuteHubCtrlRegisterAPClient(const unsigned int &aTimeout_ms)
{
  bool result = false;
  int r;
  TMsgReplyBufferEntry mrbe;
  const unsigned int totsize = ( sizeof(THubCtrlProtocolHeader) + 
                                 sizeof(THubCtrlSubHeaderAuthentication) );
  uint8_t buffer[totsize];
  THubCtrlProtocolHeader *frame = (THubCtrlProtocolHeader *)buffer;
  THubCtrlSubHeaderAuthentication *auth = (THubCtrlSubHeaderAuthentication *)(frame+1);
  

  // First operation; CHECK the timeout -> If no time left; won't do anything.
  if (aTimeout_ms == 0) return false;

  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();
  
  // Assign base parameters
  frame->protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  frame->protocol_id.total_size = totsize;
  frame->request = KHubCtrlRequestRegisterAPClient;
  frame->flags = 0x00;
  frame->subheader_count = 1;
  frame->result = 0;
  frame->message_id = ++iHubCtrlMessageSeq;
  
  // Assign authentication data
  auth->subtype = KHubCtrlSubtypeAuthentication;
  auth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
  memset(auth->sha1, 0x00, sizeof(auth->sha1));
  auth->timestamp_s = time(NULL);
         
  // Now; calculate authentication data SHA-1; via separate buffer.
  uint8_t sha1[20];
  unsigned int sha1_size = 20;
  apsecurityCalculateHMAC(sha1,
                          &sha1_size,
                          (uint8_t*)iHubCtrlSecretKey,
                          iHubCtrlSecretKeySize,
                          buffer,
                          totsize);
  assert(sha1_size == 20);
  memcpy(auth->sha1, sha1, sizeof(sha1));


  // Complete. SEND!
  r = msgreplybufferInsertMsg(&iHubCtrlMsgReplyBuffer, frame->message_id, NULL);
  assert(r == 0);

  Lock(KConnectorLockSend);
  r = protocolDispSendPacket(&iProtocolDisp, (const unsigned char*)buffer, totsize);
  Unlock(KConnectorLockSend);

  // Unexpected error outside normal program flow!
  if (r) {
    dPrintLCRed(ODERROR,"Hard network link level error encountered! Forcing the socket to close");
    CloseConnection();

    // Break flow !
    return false;
  }

  // Now, start waiting for reply.
  if (msgreplybufferWaitReply(&iHubCtrlMsgReplyBuffer, 
                              &mrbe, 
                              frame->message_id, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    EHubCtrlResult s = (EHubCtrlResult)(long unsigned int)mrbe.reply;
    switch(s) {
    case KHubCtrlResultUnknownError:
      dPrint(ODERROR,"Failed to register as GIMnetAP Client - Unknown reason!");
      break;

    case KHubCtrlResultOK:
      dPrint(ODVINFO,"Succesfully registered as GIMnetAP Client");
      result = true;
      break;

    case KHubCtrlResultIdReserved:
      dPrintLCRed(ODERROR,"Failed to register as GIMnetAP Client - ID reserved.");
      break;

    case KHubCtrlResultAuthFailed:
      dPrintLCRed(ODERROR,"Failed to register as GIMnetAP Client - Authentication failed.");
      break;

    case KHubCtrlResultAuthExpired:
      dPrintLCRed(ODERROR,"Failed to register as GIMnetAP Client - Authentication Expired.");
      break;

    }
    
  } else {
    dPrint(ODERROR,"Timeout waiting for Reply from GIMnetAP for RegisterAPClient Query - Please try again!");
    
  }

  return result;
}
//*****************************************************************************

bool Connector::ExecuteHubCtrlQueryLocalNodeId(const unsigned int &aTimeout_ms)
{ 
  int r;
  bool result = false;
  TMsgReplyBufferEntry mrbe;

  // First operation; CHECK the timeout -> If no time left; won't do anything.
  if (aTimeout_ms == 0) return false;
  
  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();
  
  // Copy parameters
  THubCtrlProtocolHeader frame;
  frame.protocol_id.protocol_number = HUBCTRLPROTOCOL_NUMBER;
  frame.protocol_id.total_size = sizeof(THubCtrlProtocolHeader);
  frame.request = KHubCtrlRequestQueryLocalNodeId;
  frame.flags = 0x00;
  frame.subheader_count = 0;
  frame.result = 0;
  frame.message_id = ++iHubCtrlMessageSeq;
  
  r = msgreplybufferInsertMsg(&iHubCtrlMsgReplyBuffer, frame.message_id, NULL);
  assert(r == 0);
  Lock(KConnectorLockSend);
  r = protocolDispSendPacket(&iProtocolDisp, 
                             (const unsigned char *)&frame, 
                             frame.protocol_id.total_size);
  Unlock(KConnectorLockSend);

  if (r) {
    dPrintLCRed(ODERROR,"Hard network link level error encountered! Forcing the socket to close");
    CloseConnection();
    
    // Break flow !
    return false;
  }
  
  if (msgreplybufferWaitReply(&iHubCtrlMsgReplyBuffer, 
                              &mrbe, 
                              frame.message_id, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    iOwnHubId = (uint32_t)(long unsigned int)mrbe.reply;
    dPrint(ODVINFO,"Query for OwnId resulted in '0x%08x'", iOwnHubId);
    result = true;

  } else {
    dPrint(ODSTATUS,"Timeout/Error waiting for Reply from GIMnetAP for QueryLocalNodeId Query - Please try again!");

  }

  return result;
}
//*****************************************************************************

bool Connector::ExecuteNameServiceRegisterName(const unsigned int &aTimeout_ms)
{
  int r;
  bool result = false;
  TMsgReplyBufferEntry mrbe;
  
  // First operation; CHECK the timeout -> If no time left; won't do anything.
  if (aTimeout_ms == 0) return false;

  Lock(KConnectorLockReceive);
  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();
  
  /////////////////////////////////////////////////////////////////////
  ///////// NOW; Attempt to register our name
  TFastProtocolHeader fphdr;
  // Assign base parameters
  fphdr.protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fphdr.protocol_id.total_size = sizeof(TFastProtocolHeader)+sizeof(SAPNameServiceFrame);
  fphdr.statusvalue = 0x00;
  fphdr.ttl = FASTPROTOCOL_DEFAULT_TTL;
  fphdr.flags = 0x00;
  fphdr.target_id = iNameServiceNodeId;
  fphdr.source_id = iOwnHubId;


  SAPNameServiceFrame nsframe;
  assert(iOwnHubName.size() < sizeof(nsframe.name));

  nsframe.service_id = GIMNETAP_SERVICE_NAMESERVICE_ID;
  nsframe.request = KAPNameServiceRequestRegister;
  nsframe.flags = iForceNameRegistration ? KAPNameServiceFlagOverride : 0x00;
  nsframe.identifier = ++iNameServiceMessageSeq;
  memset(nsframe.name, 0x00, sizeof(nsframe.name));
  strncpy(nsframe.name, iOwnHubName.c_str(), sizeof(nsframe.name)-1);
  nsframe.nodeid = iOwnHubId;
  nsframe.age_left = iNameServiceOwnNameValidity; // FIXME
  nsframe.result = 0x00;
  nsframe.reserved1 = 0x00;
  nsframe.reserved2 = 0x0000;

  r = msgreplybufferInsertMsg(&iNameServiceMsgReplyBuffer, nsframe.identifier, NULL);
  assert(r == 0);
  Lock(KConnectorLockSend);
  r = protocolDispSendPacketEx(&iProtocolDisp,
                               (const unsigned char*)&fphdr,sizeof(fphdr),
                               (const unsigned char*)&nsframe,sizeof(nsframe));
  Unlock(KConnectorLockSend);
  Unlock(KConnectorLockReceive);

  if (r != PROTOCOLDISPATCHER_OK) {
    dPrintLCRed(ODERROR,"Hard network link level error encountered! Forcing the socket to close");
    CloseConnection();
    
    // Break flow !
    return false;
  }
  
  // Now, enter a separate waiting mechanism.
  if (msgreplybufferWaitReply(&iNameServiceMsgReplyBuffer,
                              &mrbe,
                              nsframe.identifier, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    const SAPNameServiceFrame *nsrframe = (const SAPNameServiceFrame *)mrbe.reply;
    assert(nsrframe != NULL);
    
    if (nsrframe->result == KAPNameServiceResultOK) {
      dPrint(ODVINFO,"Operation OK! Succesfully registered name '%s' to nodeid: 0x%08x (Expires in %u s)",
             nsrframe->name, nsrframe->nodeid, nsrframe->age_left);
      
      // If the stored refresh interval is LONGER than the whole validity time; override the value to 2/3.
      if (nsrframe->age_left < iNameServiceRefreshIntervalMs/1000) {
        iNameServiceRefreshIntervalMs = ((nsrframe->age_left * 2000) / 3);
        dPrint(ODWARN,"Note: User specified RefreshInterval was overridden with a server enforced value %lums",
               iNameServiceRefreshIntervalMs);
      }

      // "Everything seems to be in oooorder"

      result = true;

    } else {
      dPrint(ODERROR,"Failed to Register name '%s' for local NodeId: '0x%08x': %s", 
             nsrframe->name, 
             nsrframe->nodeid,
             nsrframe->result==KAPNameServiceResultNameReserved?"Name is already reserved":
             nsrframe->result==KAPNameServiceResultInvalidName?"Invalid name given":
             "Unknown error");
      
    }
    
    delete nsrframe;
    
  } else {
    dPrint(ODSTATUS,"Timeout waiting for Reply from GIMnetAP for Register Query - Please try again!");
    
  }

  return result;
}
//*****************************************************************************

bool Connector::ExecuteNameServiceRegisterNameRefresh(const unsigned int &aTimeout_ms)
{
  return ExecuteNameServiceRegisterName(aTimeout_ms);
}
//*****************************************************************************

bool Connector::ExecuteNameServiceUnregisterName(const unsigned int &aTimeout_ms)
{
  int r;
  bool result = false;
  TMsgReplyBufferEntry mrbe;
  
  // First operation; CHECK the timeout -> If no time left; won't do anything.
  if (aTimeout_ms == 0) return false;

  Lock(KConnectorLockReceive);
  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();
  
  /////////////////////////////////////////////////////////////////////
  ///////// NOW; Attempt to register our name
  TFastProtocolHeader fphdr;
  // Assign base parameters
  fphdr.protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fphdr.protocol_id.total_size = sizeof(TFastProtocolHeader)+sizeof(SAPNameServiceFrame);
  fphdr.statusvalue = 0x00;
  fphdr.ttl = FASTPROTOCOL_DEFAULT_TTL;
  fphdr.flags = 0x00;
  fphdr.target_id = iNameServiceNodeId;
  fphdr.source_id = iOwnHubId;


  SAPNameServiceFrame nsframe;
  assert(iOwnHubName.size() < sizeof(nsframe.name));

  nsframe.service_id = GIMNETAP_SERVICE_NAMESERVICE_ID;
  nsframe.request = KAPNameServiceRequestUnregister;
  nsframe.flags = 0x00;
  nsframe.identifier = ++iNameServiceMessageSeq;
  memset(nsframe.name, 0x00, sizeof(nsframe.name));
  strncpy(nsframe.name, iOwnHubName.c_str(), sizeof(nsframe.name)-1);
  nsframe.nodeid = iOwnHubId;
  nsframe.age_left = iNameServiceOwnNameValidity; // FIXME
  nsframe.result = 0x00;
  nsframe.reserved1 = 0x00;
  nsframe.reserved2 = 0x0000;

  r = msgreplybufferInsertMsg(&iNameServiceMsgReplyBuffer, nsframe.identifier, NULL);
  assert(r == 0);
  Lock(KConnectorLockSend);
  r = protocolDispSendPacketEx(&iProtocolDisp,
                               (const unsigned char*)&fphdr,sizeof(fphdr),
                               (const unsigned char*)&nsframe,sizeof(nsframe));
  Unlock(KConnectorLockSend);
  Unlock(KConnectorLockReceive);
  
  if (r) {
    dPrint(ODTEST,"Failed to send message, no sense to wait for reply");
    return false;
  }
  


  // Now, enter a separate waiting mechanism.
  if (msgreplybufferWaitReply(&iNameServiceMsgReplyBuffer,
                              &mrbe,
                              nsframe.identifier, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    const SAPNameServiceFrame *nsrframe = (const SAPNameServiceFrame *)mrbe.reply;
    assert(nsrframe != NULL);
    
    if (nsrframe->result == KAPNameServiceResultOK) {
      dPrint(ODVINFO,"Operation OK! Succesfully Unregistered name '%s'",
             nsrframe->name);
      result = true;

    } else {
      dPrint(ODWARN,"Failed to Unregister name '%s': %s", 
             nsrframe->name, 
             nsrframe->result==KAPNameServiceResultNameReserved?"Name is already reserved":
             nsrframe->result==KAPNameServiceResultInvalidName?"Invalid name given":
             "Unknown error");
      
    }
    
    delete nsrframe;
    
  } else {
    dPrint(ODSTATUS,"Timeout waiting for Reply from GIMnetAP for Unregister Query - Please try again!");
    
  }

  return result;
}
//*****************************************************************************

bool Connector::ExecuteMulticastServiceSubscribe(const unsigned int &aTimeout_ms)
{
  int r;
  bool result = true;
  TMsgReplyBufferEntry mrbe;

  // First operation; CHECK the timeout -> If no time left; won't do anything.
  if (aTimeout_ms == 0) return false;
  
  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();

  // Const
  const unsigned int multicast_buffer_size = ( sizeof(TFastProtocolHeader) + 
                                               sizeof(TAPMulticastFrame) + 
                                               sizeof(TAPMulticastCtrl) );
  uint8_t multicast_buffer[multicast_buffer_size];

  // Init to Zero (Allows leaving the Zero initialization fields out from the following code)
  memset(multicast_buffer, 0x00, multicast_buffer_size);


  // Create easier-to-use pointers
  TFastProtocolHeader *fpframe = (TFastProtocolHeader*)multicast_buffer;
  TAPMulticastFrame *mcframe = (TAPMulticastFrame *)(multicast_buffer+sizeof(TFastProtocolHeader));
  TAPMulticastCtrl *mcctrl = (TAPMulticastCtrl *)(multicast_buffer+sizeof(TAPMulticastFrame)+sizeof(TFastProtocolHeader));
  
  // Setup FastProtocolFrame
  //fpframe->source_id = 0x00;
  fpframe->target_id = iMulticastNodeId;
  //fpframe->flags = 0x00;
  fpframe->ttl = FASTPROTOCOL_DEFAULT_TTL;
  //fpframe->statusvalue = 0x00;
  fpframe->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fpframe->protocol_id.total_size = multicast_buffer_size; 

  // Setup MulticastFrame
  mcframe->service_id = GIMNETAP_SERVICE_MULTICAST_ID;
  mcframe->request = KAPMulticastRequestSubscribe;
  //mcframe->flags = 0x00;
  mcframe->identifier = ++iMulticastMessageSeq;

  // Setup payload header
  mcctrl->multicast_id = iMulticastServiceDiscoveryGroupId;
  //  mcctrl->owner_key = 0x0000000000000000;
  mcctrl->access_key = iMulticastServiceDiscoveryAccessKey;
  //mcctrl->result = 0x0000000;
  
  // Send! 
  r = msgreplybufferInsertMsg(&iMulticastMsgReplyBuffer, mcframe->identifier, NULL);
  assert(r == 0);
  
  Lock(KConnectorLockSend);
  r = protocolDispSendPacket(&iProtocolDisp, 
                             (const unsigned char*)multicast_buffer,
                             multicast_buffer_size);
  Unlock(KConnectorLockSend);
  if (r) {
    dPrintLCRed(ODERROR,"Hard network link level error encountered! Forcing the socket to close");
    
    // Break flow !
    return false;
  }
  
  
  // Check reply
  //  dPrint(ODTEST,"Entering wait for reply (%u ms left)", aTimeout_ms);
  if (msgreplybufferWaitReply(&iMulticastMsgReplyBuffer,
                              &mrbe,
                              mcframe->identifier, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    const SAPMulticastFrame *mcrframe = (const SAPMulticastFrame *)mrbe.reply;
    const SAPMulticastCtrl *mcctrlrframe = (const SAPMulticastCtrl *)((char *)mrbe.reply+sizeof(SAPMulticastFrame));
    assert(mcrframe != NULL);
    
    if (mcrframe->flags & KAPMulticastFlagReply &&
        mcrframe->request == KAPMulticastRequestSubscribe &&
        mcctrlrframe->result == KAPMulticastResultOK) {
      dPrint(ODVINFO,"Operation OK! Succesfully Subscribed to MulticastGroup for ServiceDiscovery");
      result = true;

    } else {
      dPrint(ODWARN,"Failed to Subscribe to MulticastGroup %u for ServiceDiscovery: %s",
             mcctrlrframe->multicast_id,
             mcctrlrframe->result==KAPMulticastResultIdNotFound?"Requested ID not found":
             mcctrlrframe->result==KAPMulticastResultAccessDenied?"Access denied":
             "Unknown error");
      
    }
    
    delete[] mcrframe;

  } else {
    dPrint(ODSTATUS,"Timeout waiting for Reply from GIMnetAP for Multicast Subscribe Query - Please try again!");
    
  }

  return result;
}
//*****************************************************************************

bool Connector::ExecuteMulticastServiceJoinAnnouncement(const unsigned int &aTimeout_ms)
{
  bool result = true;
#warning No Join announcement implemented yet!
  return result;
}
//*****************************************************************************

bool Connector::ExecuteMulticastServiceUnsubscribe(const unsigned int &aTimeout_ms)
{
  int r;
  bool result = true;
  TMsgReplyBufferEntry mrbe;

  // First operation; CHECK the timeout -> If no time left; won't do anything.
  if (aTimeout_ms == 0) return false;
  
  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();

  // Const
  const unsigned int multicast_buffer_size = ( sizeof(TFastProtocolHeader) + 
                                               sizeof(TAPMulticastFrame) + 
                                               sizeof(TAPMulticastCtrl) );
  uint8_t multicast_buffer[multicast_buffer_size];

  // Create easier-to-use pointers
  TFastProtocolHeader *fpframe = (TFastProtocolHeader*)multicast_buffer;
  TAPMulticastFrame *mcframe = (TAPMulticastFrame *)(multicast_buffer+sizeof(TFastProtocolHeader));
  TAPMulticastCtrl *mcctrl = (TAPMulticastCtrl *)(multicast_buffer+sizeof(TAPMulticastFrame)+sizeof(TFastProtocolHeader));
  
  // Setup FastProtocolFrame
  fpframe->source_id = 0x00;
  fpframe->target_id = iMulticastNodeId;
  fpframe->statusvalue = 0x00;
  fpframe->flags = 0x00;
  fpframe->ttl = FASTPROTOCOL_DEFAULT_TTL;
  fpframe->statusvalue = 0x00;
  fpframe->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fpframe->protocol_id.total_size = multicast_buffer_size; 

  // Setup MulticastFrame
  mcframe->service_id = GIMNETAP_SERVICE_MULTICAST_ID;
  mcframe->request = KAPMulticastRequestUnsubscribe;
  mcframe->flags = 0x00;
  mcframe->identifier = ++iMulticastMessageSeq;
  
  // Setup payload header
  mcctrl->multicast_id = iMulticastServiceDiscoveryGroupId;
  mcctrl->access_key = iMulticastServiceDiscoveryAccessKey;
  mcctrl->owner_key = 0x0000000000000000;
  mcctrl->result = 0x00;
  
  // Send! 
  r = msgreplybufferInsertMsg(&iMulticastMsgReplyBuffer, mcframe->identifier, NULL);
  assert(r == 0);
  
  Lock(KConnectorLockSend);
  r = protocolDispSendPacket(&iProtocolDisp, 
                             (const unsigned char*)multicast_buffer,
                             multicast_buffer_size);
  Unlock(KConnectorLockSend);
  
  if (r) {
    dPrint(ODTEST,"Failed to send message, no sense to wait for reply.");
    // Break flow !
    return false;
  }
  


  // Check reply
  if (msgreplybufferWaitReply(&iMulticastMsgReplyBuffer,
                              &mrbe,
                              mcframe->identifier, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    const SAPMulticastFrame *mcrframe = (const SAPMulticastFrame *)mrbe.reply;
    const SAPMulticastCtrl *mcctrlrframe = (const SAPMulticastCtrl *)((char *)mrbe.reply+sizeof(SAPMulticastFrame));
    assert(mcrframe != NULL);
    
    if (mcrframe->flags & KAPMulticastFlagReply &&
        mcrframe->request == KAPMulticastRequestUnsubscribe &&
        mcctrlrframe->result == KAPMulticastResultOK) {
      dPrint(ODVINFO,"Operation OK! Succesfully Unsubscribed from MulticastGroup for ServiceDiscovery");
      result = true;
      
    } else {
      dPrint(ODWARN,"Failed to Unsubscribe from MulticastGroup %u for ServiceDiscovery: %s",
             mcctrlrframe->multicast_id,
             mcctrlrframe->result==KAPMulticastResultIdNotFound?"Requested ID not found":
             mcctrlrframe->result==KAPMulticastResultAccessDenied?"Access denied":
             "Unknown error");
      
    }
    
  } else {
    dPrint(ODSTATUS,"Timeout waiting for Reply from GIMnetAP for Multicast Unsubscribe Query - Please try again!");
    
  }

  return result;
}
//*****************************************************************************


int Connector::ExecuteClientRegistration(const unsigned int &aTimeout_ms)
{
  const double begin_s = ownTime_get_s();
  const ownTime_ms_t begin = ownTime_get_ms();
  
  bool subExec = true;
  subExec &= ExecuteHubCtrlRegisterAPClient(ownTime_get_ms_left(begin, aTimeout_ms));
  dPrint(ODVINFO, "Time after RegisterAPClient: %.2f ms", 1000.0*(ownTime_get_s()-begin_s));

  if (subExec) subExec &= ExecuteHubCtrlQueryLocalNodeId(ownTime_get_ms_left(begin, aTimeout_ms));
  dPrint(ODVINFO, "Time after QueryLocalNodeId: %.2f ms", 1000.0*(ownTime_get_s()-begin_s));

  if (subExec) subExec &= ExecuteNameServiceRegisterName(ownTime_get_ms_left(begin, aTimeout_ms));
  dPrint(ODVINFO, "Time after RegisterName: %.2f ms", 1000.0*(ownTime_get_s()-begin_s));

  if (subExec) subExec &= ExecuteMulticastServiceSubscribe(ownTime_get_ms_left(begin, aTimeout_ms));
  dPrint(ODVINFO, "Time after MulticastServiceSubscribe: %.2f ms", 1000.0*(ownTime_get_s()-begin_s));

  if (subExec) subExec &= ExecuteMulticastServiceJoinAnnouncement(ownTime_get_ms_left(begin, aTimeout_ms));
  dPrint(ODVINFO, "Time after MulticastServiceJoinAnnouncement: %.2f ms", 1000.0*(ownTime_get_s()-begin_s));

  
  dPrint(ODVINFO,"ClientRegistration: Operation completed in %.2f ms", 
         1000.0*(ownTime_get_s()-begin_s));

  return subExec ? GIMI_OK : GIMI_ERROR;
}
//*****************************************************************************

int Connector::connectToHub(const std::string &aAddress,
                            const int aPort,
                            const std::string &aOwnName,
                            const bool aForceNameRegistration,
                            const uint8_t *aHubCtrlSecretClientKey,
                            const unsigned int aHubCtrlSecretClientKeySize,
                            const int aTimeout_ms)
{
  int result = GIMI_ERROR;

  Lock(KConnectorLockReceive);

  // Assign key
  iHubCtrlSecretKeySize = aHubCtrlSecretClientKeySize;
  iHubCtrlSecretKey = new uint8_t [ iHubCtrlSecretKeySize ];
  memcpy(iHubCtrlSecretKey, aHubCtrlSecretClientKey, iHubCtrlSecretKeySize );
  
  // Verify that system is down.
  if(iIsConnected || iIsActive) {
    CloseConnection();
  }

  // If name is empty, generate a "unique" one.
  if (aOwnName.empty()) {
    long int s, us;
    ownTime_GetTimeOfDay(&s, &us);
    char newName[256];
    snprintf(newName, 255, "anonymous_%08x_%05x", (unsigned)s, (unsigned)us);
    iOwnHubName = newName;

  } else {
    iOwnHubName = aOwnName;

  }

  // Store flag for registration
  iForceNameRegistration = aForceNameRegistration;

  // Store address & port to class instance.
  iHubAddress = aAddress;
  iHubPort = aPort;


  // Do connect
  dPrint(ODVINFO,"Connecting to '%s' at port %u", iHubAddress.c_str(), iHubPort);
  Lock(KConnectorLockSend);
  result = protocolDispConnect(&iProtocolDisp, iHubAddress.c_str(), iHubPort);
  Unlock(KConnectorLockSend);
  if (result == PROTOCOLDISPATCHER_OK) {

    // Start threads - Go on with Registration.
    ExecuteThreads();
    
    // And; release Locks to allow the Execute()... family to handle
    // their own locking without Recursive Mutex support.
    Unlock(KConnectorLockReceive);

    // Now, registrate
    result = ExecuteClientRegistration(aTimeout_ms);

    if (result == GIMI_OK) {
      dPrint(ODVINFO,"GIMI instance %p connected to '%s:%u' as '%s' (0x%08x)", 
             this, aAddress.c_str(), aPort, iOwnHubName.c_str(), iOwnHubId);
      iIsConnected = true;

      
    } else {
      dPrintLCRed(ODERROR,"GIMI Instance activation failed! Fatal error!");
      CloseConnection();

    }

  } else {
    Unlock(KConnectorLockReceive);

  }
  return result;
}
//*****************************************************************************
    
int Connector::sendDataToTarget(const uint32_t aTargetId,
                                const uint8_t *aData,
                                const unsigned int aDatalen)
{
  assert(aData != NULL);
  assert(aDatalen > 0);
  assert(aTargetId != 0x00000000);
   
  // Don't do anything if not connected.
  if (!isActive() || !isConnected()) {
    dPrint(4, "Not connected to hub!");
    return GIMI_NOCONNECTION;
  }
  
  TFastProtocolHeader fphdr;
  fphdr.protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fphdr.protocol_id.total_size = sizeof(TFastProtocolHeader)+aDatalen;
  fphdr.statusvalue = 0x00000000;
  fphdr.ttl = FASTPROTOCOL_DEFAULT_TTL;
  fphdr.flags = (FASTPROTOCOL_FLAG_REPORT_ERROR|
                 FASTPROTOCOL_FLAG_REPORT_STATUS);
  fphdr.target_id = aTargetId;
  fphdr.source_id = iOwnHubId;
  
  Lock(KConnectorLockSend);
  int result = protocolDispSendPacketEx(&iProtocolDisp,
                                        (const unsigned char*)&fphdr,
                                        sizeof(fphdr),
                                        aData,
                                        aDatalen);
  Unlock(KConnectorLockSend);
  if (result != PROTOCOLDISPATCHER_OK) {
    //dPrint(ODTEST, "Failed to send data to target");
    
  } else {
    //dPrint(ODTEST,"Sending %u bytes to 0x%08x succeeded.", aDatalen, aTargetId);
    result = GIMI_OK;
    
  }
  
  return result;
}
//*****************************************************************************

int Connector::sendDataMulticast(const t_clientIdList &aTargetIds,
                                 const uint8_t* aData,
                                 const unsigned int aDatalen)
{
  assert(aData != NULL);
  assert(!aTargetIds.empty());
  assert(aDatalen > 0);

  // Don't do anything if not connected.
  if (!isActive() || !isConnected()) {
    dPrint(4, "Not connected to GIMnetAP!");
    return GIMI_NOCONNECTION;
  }

  // Take some important values; num of subscribers and total size of
  // sent frame minus size of dataLen (as payload is given to sender
  // function separately)
  const unsigned int numberOfSubscribers = aTargetIds.size();
  const unsigned int header_size = ( sizeof(TFastProtocolHeader) + 
                                     (numberOfSubscribers * sizeof(uint32_t)) );
  uint8_t buffer[header_size]; // Automatic dynamic allocation
  TFastProtocolHeader *fphdr = (TFastProtocolHeader *)(buffer);
  uint32_t *targetNodeIdPtr = (uint32_t*)(fphdr+1);

  // Construct FastProtocol header
  fphdr->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fphdr->protocol_id.total_size = header_size+aDatalen;
  fphdr->ttl = FASTPROTOCOL_DEFAULT_TTL;
  fphdr->flags = (FASTPROTOCOL_FLAG_REPORT_ERROR |
                  FASTPROTOCOL_FLAG_REPORT_STATUS |
                  FASTPROTOCOL_FLAG_MODE_EXPLICIT_MULTICAST);
  fphdr->target_count = numberOfSubscribers;
  fphdr->source_id = iOwnHubId;
  

  // Copy multicast targets
  for(t_clientIdList::const_iterator ci = aTargetIds.begin();
      ci != aTargetIds.end();
      ++ci) {
    *targetNodeIdPtr = *ci;
    ++targetNodeIdPtr;
  }

  // Send!
  Lock(KConnectorLockSend);
  const int result = protocolDispSendPacketEx(&iProtocolDisp,
                                              (const unsigned char*)buffer,
                                              header_size,
                                              aData,
                                              aDatalen);
  Unlock(KConnectorLockSend);

  /*  if (result != PROTOCOLDISPATCHER_OK) {
    //dPrint(ODTEST,"WUT!? Failed to send!");
    
  } else {
    //dPrint(ODTEST,"Sending %u bytes of payload to %u subscribers succeeded.", 
    //       aDatalen, numberOfSubscribers);
    
    }*/
  
  return result;
}
//*****************************************************************************

int Connector::findClientId(uint32_t &clientId,
                            const std::string &clientName,
                            const unsigned int &aTimeout_ms)
{
  int result = GIMI_OK;

  // Don't do anything if not connected.
  if (!isActive()) {
    dPrint(4, "Not connected to hub!");
    return GIMI_NOCONNECTION;

  } else if (!clientName.size()) {
    dPrint(ODERROR,"Invalid name given. '%s' not allowed", clientName.c_str());
    return GIMI_INVALIDNAME;
    
  }
  
  //  dPrint(ODTEST,"Query ID for '%s'", clientName.c_str());


  // Check cache.
  SNameCacheEntry nce;
  if (iNameCache.GetNameCacheEntryByName(nce, clientName)) {
    // Found from cache.
    clientId = nce.nodeid;

  } else {
    const uint32_t mid = sendNameServiceQuery(KAPNameServiceRequestIdByName, 
                                              clientName, 0, 0);
    
    
    //  Wait
    TMsgReplyBufferEntry mrbe;
    if (msgreplybufferWaitReply(&iNameServiceMsgReplyBuffer, 
                                &mrbe, mid, aTimeout_ms) == 0) {
      const SAPNameServiceFrame *nsframe = (const SAPNameServiceFrame *)mrbe.reply;
      assert(nsframe != NULL);
      
      if (nsframe->result == KAPNameServiceResultOK) {
        dPrint(ODVINFO,"Operation OK! Got ID '0x%08x' for name '%s' (Expires in %u s)",
               nsframe->nodeid, nsframe->name, nsframe->age_left);


        clientId = nsframe->nodeid;
        
        // Insert to cache.
        const bool insert_result = iNameCache.InsertNameCacheEntry(SNameCacheEntry(nsframe->name, 
                                                                                   nsframe->nodeid, 
                                                                                   gim::time(true, 
                                                                                             nsframe->age_left * 1000)));
        assert(insert_result == true);

        
      } else {
        dPrint(ODWARN,"Failed to Get ID for name '%s': %s", 
               nsframe->name, 
               nsframe->result==KAPNameServiceResultEntryNotFound?"Entry not found from NameService":
               nsframe->result==KAPNameServiceResultInvalidName?"Invalid name given":"Unknown error");
        clientId = 0x00000000;
        result = GIMI_INVALIDNAME;
      }
      
      
      delete nsframe;
    } else {
      dPrintLCRed(ODWARN,"No answer from Nameservice in %u ms! Expect some serious trouble!", aTimeout_ms);
      
    }
  }
    
  return result;
}
//*****************************************************************************

int Connector::findClientName(std::string& clientName,
                              const uint32_t &clientId,
                              const unsigned int &aTimeout_ms)
{
  int result = GIMI_OK;

  // Don't do anything if not connected.
  if (!isActive()) {
    dPrint(4, "Not connected to hub!");
    return GIMI_NOCONNECTION;

  } else if (!clientId) {
    dPrint(ODERROR,"Invalid ID given. '%08x' not allowed", clientId);
    return GIMI_INVALIDNAME;
    
  }
  
  clientName.clear();

  // Check cache.
  SNameCacheEntry nce;
  if (iNameCache.GetNameCacheEntryById(nce, clientId)) {
    // Found from cache.
    clientName = nce.name;

  } else {
    const uint32_t mid = sendNameServiceQuery(KAPNameServiceRequestNameById, 
                                              "", clientId, 0);

    //  Wait
    TMsgReplyBufferEntry mrbe;
    if (msgreplybufferWaitReply(&iNameServiceMsgReplyBuffer, 
                                &mrbe, mid, aTimeout_ms) == 0) {
      const SAPNameServiceFrame *nsframe = (const SAPNameServiceFrame *)mrbe.reply;
      assert(nsframe != NULL);
      
      if (nsframe->result == KAPNameServiceResultOK) {
        //        dPrint(ODTEST,"Operation OK! Got Name '%s' for ID '0x%08x' (Expires in %u s)",
        //nsframe->name, nsframe->nodeid, nsframe->age_left);

        clientName = nsframe->name;
        
        // Insert to cache.
        const bool insert_result = iNameCache.InsertNameCacheEntry(SNameCacheEntry(nsframe->name, 
                                                                                   nsframe->nodeid, 
                                                                                   gim::time(true, 
                                                                                             nsframe->age_left * 1000)));
        assert(insert_result == true);
        
      } else {
        dPrint(ODVINFO,"Failed to Get Name for ID '%08x': %s", 
               nsframe->nodeid, 
               nsframe->result==KAPNameServiceResultEntryNotFound?"Entry not found from NameService":
               nsframe->result==KAPNameServiceResultInvalidName?"Invalid name given":"Unknown error");

        result = GIMI_INVALIDNAME;
      }
      
      
      delete nsframe;
    } else {
      dPrintLCRed(ODWARN, "No answer from Nameservice for MsgIdentifier %u in %u ms! Expect some serious trouble!", 
                  mid, aTimeout_ms);

      result = GIMI_INVALIDCLIENT;
    }
  }

  return result;
}
//*****************************************************************************

uint32_t Connector::sendNameServiceQuery(const EAPNameServiceRequest &aRequest,
                                         const std::string &aName,
                                         const uint32_t &aNodeId,
                                         const uint32_t &aAge_Left)
{
  uint32_t result = 0;

  TFastProtocolHeader fphdr;
  // Assign base parameters
  fphdr.protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fphdr.protocol_id.total_size = sizeof(TFastProtocolHeader)+sizeof(SAPNameServiceFrame);
  fphdr.statusvalue = 0x00000000;
  fphdr.ttl = FASTPROTOCOL_DEFAULT_TTL;
  fphdr.flags = 0x00;
  fphdr.target_id = iNameServiceNodeId;
  fphdr.source_id = iOwnHubId;

  SAPNameServiceFrame nsframe;
  nsframe.service_id = GIMNETAP_SERVICE_NAMESERVICE_ID;
  nsframe.request = aRequest;
  nsframe.flags = 0x00;
  nsframe.identifier = ++iNameServiceMessageSeq;
  strncpy(nsframe.name, aName.c_str(), sizeof(nsframe.name));
  nsframe.name[sizeof(nsframe.name)-1] = '\0';
  nsframe.nodeid = aNodeId;
  nsframe.age_left = aAge_Left; // FIXME
  nsframe.result = 0x00;
  nsframe.reserved1 = 0x00;
  nsframe.reserved2 = 0x0000;


  // Constructed.
  int r = msgreplybufferInsertMsg(&iNameServiceMsgReplyBuffer, nsframe.identifier, NULL);
  Lock(KConnectorLockSend);
  r &= protocolDispSendPacketEx(&iProtocolDisp,
                                (const unsigned char*)&fphdr,
                                sizeof(fphdr),
                                (const unsigned char*)&nsframe,
                                sizeof(nsframe));
  Unlock(KConnectorLockSend);
  if (r != PROTOCOLDISPATCHER_OK) {
    //    dPrint(ODTEST,"WUT!? Failed to send NameServiceQuery?");
    
  } else {
    result = nsframe.identifier;

  }
 
  return result;
}
//*****************************************************************************

inline uint32_t Connector::GetMulticastGroupIdByMulticastType(const EMulticastType &aMulticastType)
{
  uint32_t result = 0;
  
  switch(aMulticastType) {
  case KMulticastTypeServiceDiscovery:
    result = iMulticastServiceDiscoveryGroupId;
    break;
  default:
    dPrintLCRed(ODTEST,"WARNING; Unidentified MulticastType requested! (%u)", 
           aMulticastType);
    break;
    
  }

  return result;
}
//*****************************************************************************


bool Connector::sendDataByServerMulticast(unsigned int &aDeliveredCount,
                                          const uint8_t *aData, const unsigned int &aLength,
                                          const EMulticastType &aMulticastType,
                                          const unsigned int aTimeout_ms)
{
  bool result = false;
  TMsgReplyBufferEntry mrbe;
  // Store starting time for this operation.
  const ownTime_ms_t begin = ownTime_get_ms();

  // Const
  const unsigned int multicast_buffer_header_size = ( sizeof(TFastProtocolHeader) + 
                                                      sizeof(TAPMulticastFrame) + 
                                                      sizeof(TAPMulticastPayload) );
  const unsigned int multicast_buffer_size = ( multicast_buffer_header_size + aLength );
  uint8_t * multicast_buffer = (uint8_t *) new uint8_t [multicast_buffer_size];
  assert(multicast_buffer != NULL);

  // Zero header region
  memset(multicast_buffer, 0x00, multicast_buffer_header_size);

  // Create easier-to-use pointers
  TFastProtocolHeader *fpframe = (TFastProtocolHeader*)multicast_buffer;
  TAPMulticastFrame *mcframe = (TAPMulticastFrame *)(multicast_buffer+sizeof(TFastProtocolHeader));
  TAPMulticastPayload *mcpl = (TAPMulticastPayload *)(multicast_buffer+sizeof(TAPMulticastFrame)+sizeof(TFastProtocolHeader));
  
  // Setup FastProtocolFrame
  //  fpframe->source_id = 0x00;
  fpframe->target_id = iMulticastNodeId;
  //fpframe->flags = 0x00;
  fpframe->ttl = FASTPROTOCOL_DEFAULT_TTL;
  //fpframe->statusvalue = 0x00;
  fpframe->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fpframe->protocol_id.total_size = multicast_buffer_size; 

  // Setup MulticastFrame
  mcframe->service_id = GIMNETAP_SERVICE_MULTICAST_ID;
  mcframe->request = KAPMulticastRequestMulticast;
  mcframe->flags = KAPMulticastFlagWantDeliveryCount;                ///< Flags.
  //  mcframe->identifier = 0x00;

  // Setup payload header
  mcpl->multicast_id = GetMulticastGroupIdByMulticastType(aMulticastType);
  mcpl->access_key = iMulticastServiceDiscoveryAccessKey;
  
  // copy payload
  memcpy(mcpl->payload, aData, aLength);


  // Lock!
  Lock(KConnectorLockReceive);
  mcframe->identifier = ++iMulticastMessageSeq;          
  Unlock(KConnectorLockReceive);

  // Insert to buffer
  int r = msgreplybufferInsertMsg(&iMulticastMsgReplyBuffer, mcframe->identifier, NULL);
  assert(r == 0);
  
  Lock(KConnectorLockSend);
  r = protocolDispSendPacket(&iProtocolDisp, 
                             (const unsigned char*)multicast_buffer,
                             multicast_buffer_size);
  Unlock(KConnectorLockSend);
  assert(r == 0);

  
  // Wait!
  if (msgreplybufferWaitReply(&iMulticastMsgReplyBuffer, 
                              &mrbe, 
                              mcframe->identifier, 
                              ownTime_get_ms_left(begin, aTimeout_ms)) == 0) {
    // Caaast.
    uint8_t *payload = (uint8_t *)mrbe.reply;
    const SAPMulticastFrame *mcrframe = (const SAPMulticastFrame *)payload;
    const SAPMulticastCtrl *mcrctrl = (const SAPMulticastCtrl *)(payload+sizeof(SAPMulticastFrame));
    

    if (mcrframe->flags & (KAPMulticastFlagWantDeliveryCount|KAPMulticastFlagReply) &&
        mcrframe->request == KAPMulticastRequestMulticast) {
      // Is reply & was WantDelivery & was RequestMulticast
      //dPrint(ODTEST,"Multicast was delivered (Or assumed to be...) to %u nodes",
      //       mcrctrl->result);
      aDeliveredCount = mcrctrl->result;

      result = true;

    } else {
      dPrint(ODTEST,"WARNING: Got reply but didn't match requirements!");

    }

    

    delete[] payload;


  }


  // Free temporary buffer.
  delete[] multicast_buffer;

  return result;
}
//*****************************************************************************

void Connector::NameRegistrationRefresh(const unsigned int &aTimeout_ms)
{
  const gim::time now(true);
  //dPrint(ODTEST,"Compare: %s > %s?", now.toString().c_str(), iNameServiceNextRefreshTime.toString().c_str());
  if (iIsConnected && now > iNameServiceNextRefreshTime) {
    // Do name update.
    if (!ExecuteNameServiceRegisterNameRefresh(aTimeout_ms)) {
      dPrintLCRed(ODERROR,"Failed to refresh name registration for '%s'!",
                  iOwnHubName.c_str());
      
    } else {
      dPrintLCGreen(ODVINFO,"Succesfully re-registered name: '%s'", 
                    iOwnHubName.c_str());
    
    }

    // Set next refresh time to current time + interval_ms
    iNameServiceNextRefreshTime = gim::time(true, iNameServiceRefreshIntervalMs);
  }

}
//*****************************************************************************

int Connector::GetCongestionLevel(const uint32_t &aNodeId)
{
  int level = 0;
  Lock(KConnectorLockReceive);
  ITERATOR(iCongestionLevelArray) iter = iCongestionLevelArray.find(aNodeId);
  if ( iter != iCongestionLevelArray.end()) {
    level = iter->second;
    iter->second = 0;
    //    iCongestionLevelArray.erase(iter);
  }
  Unlock(KConnectorLockReceive);  

  return level;
}
//*****************************************************************************

bool Connector::GetError(const uint32_t &aNodeId)
{
  bool error = false;
  Lock(KConnectorLockReceive);
  ITERATOR(iErrorArray) iter = iErrorArray.find(aNodeId);
  if ( iter != iErrorArray.end()) {
    error = iter->second;
    iter->second = false;
    //    iErrorArray.erase(iter);
  }
  Unlock(KConnectorLockReceive);  

  return error;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

void Connector::processNameServiceMessage(const TFastProtocolHeader *,
                                          const uint8_t * &payload,
                                          const unsigned int &payloadsize)
{
  const SAPNameServiceFrame *nsframe = (const SAPNameServiceFrame *)payload;
      
  if (payloadsize == sizeof(SAPNameServiceFrame)) {
    if (nsframe->flags & KAPNameServiceFlagReply) {
      SAPNameServiceFrame *newnsframe = new SAPNameServiceFrame();
      *newnsframe = *nsframe;

      const int r = msgreplybufferNotifyReply(&iNameServiceMsgReplyBuffer, 
                                              nsframe->identifier, 
                                              (void*)(newnsframe));
      assert( r == 0 );
      
    }
  }

}
//*****************************************************************************

void Connector::processMulticastServiceMessage(const TFastProtocolHeader *,
                                               const uint8_t * &payload,
                                               const unsigned int &payloadsize)
{ 
  const SAPMulticastFrame *mcframe = (const SAPMulticastFrame *)payload;
  const unsigned int reply_size = sizeof(SAPMulticastFrame)+sizeof(SAPMulticastCtrl);

  //dPrint(ODTEST,"Processing MulticastMessage!");
  
  if (payloadsize == reply_size) {
    if (mcframe->flags & KAPMulticastFlagReply) {
      uint8_t *buf = new uint8_t [reply_size];
      memcpy(buf, payload, reply_size);
      
      const int r = msgreplybufferNotifyReply(&iMulticastMsgReplyBuffer, 
                                              mcframe->identifier, 
                                              (void*)(buf));
      assert( r == 0 );
      
    } else {
      dPrint(ODTEST,"Warning: Received Multicast frame which is not a reply frame, what should I do with it???");

    }
  } else {
    dPrint(ODTEST,"Warning: Received Multicast (AP) frame had size mismatch (Wanted: %ubytes, got %ubytes)",
           reply_size, payloadsize);

  }
 
}
//*****************************************************************************

int Connector::processFastProtocolGIMIPayload(const TFastProtocolHeader *frame,
                                              const uint8_t * &payload,
                                              const unsigned int &payloadsize)
{
  int result = GIMI_ERROR;
  assert(frame != NULL);

  const uint32_t nodeid = frame->source_id;
  //dPrint(ODTEST,"Received frame source id: %08x", nodeid);
  
  if (frame->flags & FASTPROTOCOL_FLAG_ERROR) {
    // FIXME: Nothing done with the error type.
    //if (frame->flags & FASTPROTOCOL_FLAG_ERROR_UNREACHABLE) {
    
    //dPrint(ODTEST, "Received error unreachable from hub about client 0x%08x.", nodeid);
    const int nRemoved = iGIMICorePtr->subscribers.removeSubscriber(nodeid);
    if (nRemoved > 0) {
      dPrint(ODTEST, "Removed client 0x%08x from %d services because it was unreachable.",
             nodeid, nRemoved);
    }
    
    // dPrint(ODTEST,"Clean up NameCache entry concerning ID frame->source_id
    //dPrint(ODTEST,"Attempting to clear out name for ID %08x", frame->source_id);
    if (!iNameCache.DeleteEntryById(frame->source_id)) {
      //dPrint(ODTEST,"WARNING: Error reply from node which is not in the namecache!");
    }
    
    Lock(KConnectorLockReceive);
    ITERATOR(iErrorArray) iter = iErrorArray.find(nodeid);
    if ( iter == iErrorArray.end()) {
      iErrorArray[nodeid] = true;
      dPrint(ODTEST,"Inserted first Error entry for node %08x", nodeid);
      
    } else if (!iter->second) {
      iter->second = true;
      
    }
    Unlock(KConnectorLockReceive);
    
    
    // OK in any case.
    result = GIMI_OK;
    
  } else if (frame->flags & FASTPROTOCOL_FLAG_STATUS) {
    if (frame->flags & FASTPROTOCOL_FLAG_STATUS_CONGESTION) {
      //      dPrint(ODTEST, "Received congestion report from hub concerning source node 0x%08x", nodeid);
      Lock(KConnectorLockReceive);
      ITERATOR(iCongestionLevelArray) iter = iCongestionLevelArray.find(nodeid);
      if ( iter == iCongestionLevelArray.end()) {
        iCongestionLevelArray[nodeid] = 1;
        //dPrint(ODTEST,"Inserted first Congestion entry for node %08x", nodeid);
      } else {
        iter->second += 1;
        //dPrint(ODTEST,"Entry %08x congestion now at %d", nodeid, iter->second);
      }
      Unlock(KConnectorLockReceive);

      result = GIMI_OK;
      
    } else {
      dPrint(ODTEST, "Received unknown status report from hub.");
      result = GIMI_INVALIDTYPE;

    }

  } else if (payloadsize > 1) { // Not an error or statusreport, payload.
    //dPrint(ODTEST, "Received %u bytes of GIMI-payload from nodeid 0x%08x",
    //payloadsize, nodeid);
    iGIMICorePtr->handleGimiMessage(payload, payloadsize, nodeid);
    result = GIMI_OK;
  }
  
  return result;
}
//*****************************************************************************

void Connector::fastProtocolCallback(const uint8_t *data,
                                     const int aSize)
{
  // This function gets called when a FastProtocol packet is received
  // from AP.
  const unsigned int size = (unsigned int)aSize;
  const TFastProtocolHeader *frame = (const TFastProtocolHeader *)data;
  const uint8_t *payload = (const uint8_t *)(data+sizeof(TFastProtocolHeader));
  const unsigned int payloadsize = size - sizeof(TFastProtocolHeader);

  //dPrintLCGreen(ODTEST,"Received %u bytes from %08x", size, frame->source_id);

  if (size >= sizeof(TFastProtocolHeader) &&
      size == frame->protocol_id.total_size) {
    // First; check the subprotocol.
    const uint16_t *service_id = (const uint16_t *)(frame+1);
    switch(*service_id) {
    case GIMNETAP_SERVICE_NAMESERVICE_ID:
      //      dPrint(ODTEST,"Received NameService ServiceMessage!");
      processNameServiceMessage(frame, payload, payloadsize);
      break;

    case GIMNETAP_SERVICE_MULTICAST_ID:
      //dPrint(ODTEST,"Received Multicast ServiceMessage!");
      processMulticastServiceMessage(frame, payload, payloadsize);
      break;

    default:
      //dPrint(ODTEST,"Received Data Message from %08x! (Passing to GIMI processing)", frame->source_id);
      processFastProtocolGIMIPayload(frame, payload, payloadsize);
      break;
    }

  } else {
    dPrint(ODWARN,"Warning: Received invalid FastProtocol frame! "
           "(size = %u bytes, frame->protocol_id.total_size == %u bytes)",
           size, frame->protocol_id.total_size);
    
  }

}
//*****************************************************************************

void Connector::hubctrlProtocolCallback(const uint8_t *data,
                                        const int aSize)
{
  // This function gets called when a HubCtrlProtocol packet is received
  // from AP.
  const unsigned int size = (unsigned int)aSize;
  TProtocolDisp *link = &iProtocolDisp;
  const THubCtrlProtocolHeader *frame = (const THubCtrlProtocolHeader *)data;
  
  if (size >= sizeof(THubCtrlProtocolHeader) &&
      size == frame->protocol_id.total_size &&
      frame->protocol_id.protocol_number == HUBCTRLPROTOCOL_NUMBER) {

    if (frame->flags & KHubCtrlFlagReply) {
      // Protocol size match; We have enough data to process call
      if (frame->request == KHubCtrlRequestQueryLocalNodeId) {

        // Process QueryLocalNodeIdReply
        processQueryLocalNodeIdReply(frame, size, link);

      } else if (frame->request == KHubCtrlRequestRegisterAPClient) {
        // Process RegisterAPClienteReply
        processRegisterAPClientReply(frame, size, link); 
        
      } else {
        dPrint(ODWARN,"Unknown Reply.");
        
      }
      
    } else {
      // Not a reply.
      if (frame->request == KHubCtrlRequestClientEcho) {

        // Send AS-IS back to AP.
        Lock(KConnectorLockSend);
        protocolDispSendPacket(link, (const unsigned char*)data, size);
        Unlock(KConnectorLockSend);
        
        // Sent
        dPrint(ODTEST,"Replied to ClientEcho!");
      } else {
        dPrintLCRed(ODTEST,"WARNING: Received HubCtrl message which has no handling!");
      
      }
    }
  } else {
    dPrint(ODWARN,"Warning: Received invalid HubCtrlProtocol frame!");
    
  } 
}
//*****************************************************************************

void Connector::processQueryLocalNodeIdReply(const THubCtrlProtocolHeader *frame,
                                             const unsigned int size,
                                             TProtocolDisp *)
{
  if (size == sizeof(THubCtrlProtocolHeader)+sizeof(THubCtrlSubHeaderNodeInfo)) {
    const THubCtrlSubHeaderNodeInfo *ni = (THubCtrlSubHeaderNodeInfo *)(frame+1);
    
    dPrint(ODVINFO,"Own address for this module is '0x%08x' (Link delay: %u us)",
           ni->nodeid, ni->link_delay_us);

    msgreplybufferNotifyReply(&iHubCtrlMsgReplyBuffer, frame->message_id, (void*)(long int)(ni->nodeid));
    
  } else {
    dPrint(ODWARN,"QueryLocalNodeId: Invalid reply!");

  }
}
//*****************************************************************************

void Connector::processRegisterAPClientReply(const THubCtrlProtocolHeader *frame,
                                             const unsigned int size,
                                             TProtocolDisp *)
{
  const uint8_t *data = (const uint8_t *)frame;
  // RegisterAP received. Introduce a few variables to make
  // code more readable (Compiler should optimize these out)
  const unsigned int bsize = sizeof(THubCtrlProtocolHeader);
  const unsigned int asize = sizeof(THubCtrlSubHeaderAuthentication);
  EHubCtrlResult regresult = KHubCtrlResultUnknownError;
 
  if (size < bsize) {
    dPrint(ODTEST,"Too small reply frame!");
    return;
  }

  dPrint(ODVINFO,"Received a reply for RegisterAPClient Id=%u, operation %s",
         frame->message_id,
         frame->result==KHubCtrlResultOK ? "was a success!" :
         frame->result==KHubCtrlResultIdReserved ? "failed due to reserved ID." :
         frame->result==KHubCtrlResultAuthExpired ? "failed due to expired authorization." :
         "produced an unknown error!");
  
  // First, assign AP provided answer ( At this point, we assume the auth is OK)
  regresult = (EHubCtrlResult)frame->result;
                                         
  // Next, check that authentication exists and is valid.
  if (size == bsize+asize) {
    const THubCtrlSubHeaderAuthentication *auth = (const THubCtrlSubHeaderAuthentication *)(data+bsize); 

    const int a = hubctrlcommonAuthenticateMessage(data, size, auth, 
                                                   iHubCtrlSecretKey, 
                                                   iHubCtrlSecretKeySize, 
                                                   iHubCtrlMessageValidySeconds);
    if (a == 0) {
      dPrint(ODVINFO,"RegisterAPClient Reply Id=%u was succesfully Authenticated",frame->message_id);
 
    } else if (a == 1) {
      dPrint(ODVINFO,"RegisterAPClient Reply Id=%u, received reply had expired!", frame->message_id);
      regresult = KHubCtrlResultAuthExpired;
      
    } else {
      dPrint(ODVINFO,"RegisterAPClient Reply Id=%u failed to Authenticate!!!", frame->message_id);
      regresult = KHubCtrlResultAuthFailed;

    }

  } else if (size == bsize) {
    dPrint(ODVINFO,"RegisterAPClient Reply Id=%u contained no additional headers! (No Auth - Auth Failed)",
           frame->message_id);
    regresult = KHubCtrlResultAuthFailed;

  }

  // Abusing the reply pointer with single value ;)
  msgreplybufferNotifyReply(&iHubCtrlMsgReplyBuffer, frame->message_id, (void*)regresult);
}
//*****************************************************************************

void Connector::hubctrlProtocolCallbackFunc(int protocol_number,
                                            const unsigned char *data,
                                            int size,
                                            void *userPtr)
{
  assert(protocol_number == HUBCTRLPROTOCOL_NUMBER);
  Connector *c = reinterpret_cast<Connector *>(userPtr);
  c->hubctrlProtocolCallback(data, size);
}
//*****************************************************************************

void Connector::fastProtocolCallbackFunc(int protocol_number,
                                         const unsigned char *data,
                                         int size,
                                         void *userPtr)
{
  assert(protocol_number == FASTPROTOCOL_NUMBER);
  Connector *c = reinterpret_cast<Connector *>(userPtr);
  c->fastProtocolCallback(data, size);
}
//*****************************************************************************

int Connector::ThreadFunction(const int)
{
  return DataReceiverThreadFunc();
}
//*****************************************************************************

int Connector::DataReceiverThreadFunc()
{
  // Block all signals to this thread.
#ifndef WINDOWS_OS
  sigset_t fullmask;
  sigfillset(&fullmask);
  pthread_sigmask(SIG_BLOCK, &fullmask, NULL);
#endif

  SetCurrentThreadCancelState(ownThreadCancelStateEnable);
  
  while (!IsRequestTermination()) {
    if (protocolDispProcess(&iProtocolDisp, 24*60*60*1000)) {
      CloseConnection();
      break;
    }
  }
  
  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

